﻿using System.Runtime.InteropServices;
using MathS = System.Math;

#if F32
	using Num = System.Single;
	namespace Reign.Core.MathF32
#endif
#if F64
	using Num = System.Double;
	namespace Reign.Core.MathF64
#endif
#if I32
	using Num = System.Int32;
	namespace Reign.Core.MathI32
#endif
{
	[StructLayout(LayoutKind.Sequential)]
	public struct Matrix3
	{
		#region Properties
		public Vector3 X, Y, Z;
		#endregion

		#region Constructors
		public Matrix3(Vector3 x, Vector3 y, Vector3 z)
		{
			X = x;
			Y = y;
			Z = z;
		}

		public Matrix3(Num value)
		{
			X = new Vector3(value);
			Y = new Vector3(value);
			Z = new Vector3(value);
		}

		public Matrix3(Vector3 z, Vector3 y)
		{
			Z = z.Normalize();
			X = y.Cross(Z).Normalize();
			Y = Z.Cross(X);
		}

		public Matrix3(Vector4 quaternion)
		{
			Num X2 = quaternion.X*quaternion.X*Math.Two, Y2 = quaternion.Y*quaternion.Y*Math.Two, Z2 = quaternion.Z*quaternion.Z*Math.Two;
			Num XY = quaternion.X*quaternion.Y*Math.Two, XZ = quaternion.X*quaternion.Z*Math.Two, XW = quaternion.X*quaternion.W*Math.Two;
			Num YZ = quaternion.Y*quaternion.Z*Math.Two, YW = quaternion.Y*quaternion.W*Math.Two;
			Num ZW = quaternion.Z*quaternion.W*Math.Two;
			X = -new Vector3(Math.One-Y2-Z2, XY-ZW, XZ+YW);
			Y = new Vector3(XY+ZW, Math.One-X2-Z2, YZ-XW);
			Z = -new Vector3(XZ-YW, YZ+XW, Math.One-X2-Y2);
		}
		#endregion

		#region Operators
		// +
		public static Matrix3 operator+(Matrix3 p1, Matrix3 p2) {return new Matrix3(p1.X+p2.X, p1.Y+p2.Y, p1.Z+p2.Z);}
		public static Matrix3 operator+(Matrix3 p1, Num p2) {return new Matrix3(p1.X+p2, p1.Y+p2, p1.Z+p2);}
		public static Matrix3 operator+(Num p1, Matrix3 p2) {return new Matrix3(p1+p2.X, p1+p2.Y, p1+p2.Z);}
		// -
		public static Matrix3 operator-(Matrix3 p1, Matrix3 p2) {return new Matrix3(p1.X-p2.X, p1.Y-p2.Y, p1.Z-p2.Z);}
		public static Matrix3 operator-(Matrix3 p1, Num p2) {return new Matrix3(p1.X-p2, p1.Y-p2, p1.Z-p2);}
		public static Matrix3 operator-(Num p1, Matrix3 p2) {return new Matrix3(p1-p2.X, p1-p2.Y, p1-p2.Z);}
		// *
		public static Matrix3 operator*(Matrix3 p1, Matrix3 p2) {return new Matrix3(p1.X*p2.X, p1.Y*p2.Y, p1.Z*p2.Z);}
		public static Matrix3 operator*(Matrix3 p1, Num p2) {return new Matrix3(p1.X*p2, p1.Y*p2, p1.Z*p2);}
		public static Matrix3 operator*(Num p1, Matrix3 p2) {return new Matrix3(p1*p2.X, p1*p2.Y, p1*p2.Z);}
		// /
		public static Matrix3 operator/(Matrix3 p1, Matrix3 p2) {return new Matrix3(p1.X/p2.X, p1.Y/p2.Y, p1.Z/p2.Z);}
		public static Matrix3 operator/(Matrix3 p1, Num p2) {return new Matrix3(p1.X/p2, p1.Y/p2, p1.Z/p2);}
		public static Matrix3 operator/(Num p1, Matrix3 p2) {return new Matrix3(p1/p2.X, p1/p2.Y, p1/p2.Z);}
		// ==
		public static bool operator==(Matrix3 p1, Matrix3 p2) {return (p1.X==p2.X && p1.Y==p2.Y && p1.Z==p2.Z);}
		public static bool operator!=(Matrix3 p1, Matrix3 p2) {return (p1.X!=p2.X || p1.Y!=p2.Y || p1.Z!=p2.Z);}
		#endregion

		#region Methods
		public override bool Equals(object obj)
		{
			return obj != null && (Matrix3)obj == this;
		}

		public override string ToString()
		{
			return string.Format("{0} : {1} : {2} : {3}", X, Y, Z);
		}

		public override int GetHashCode()
		{
			return base.GetHashCode();
		}

		public static Matrix3 Identity()
		{
			return new Matrix3(new Vector3(1,0,0), new Vector3(0,1,0), new Vector3(0,0,1));
		}

		public Matrix3 Transpose()
		{
			return new Matrix3(new Vector3(X.X, Y.X, Z.X), new Vector3(X.Y, Y.Y, Z.Y), new Vector3(X.Z, Y.Z, Z.Z));
		}

		public Matrix3 Abs()
		{
			return new Matrix3(X.Abs(), Y.Abs(), Z.Abs());
		}
	
		public Matrix3 Multiply(Matrix3 matrix)
		{
			return new Matrix3(
				new Vector3((matrix.X.X*X.X) + (matrix.X.Y*Y.X) + (matrix.X.Z*Z.X), (matrix.X.X*X.Y) + (matrix.X.Y*Y.Y) + (matrix.X.Z*Z.Y), (matrix.X.X*X.Z) + (matrix.X.Y*Y.Z) + (matrix.X.Z*Z.Z)),
				new Vector3((matrix.Y.X*X.X) + (matrix.Y.Y*Y.X) + (matrix.Y.Z*Z.X), (matrix.Y.X*X.Y) + (matrix.Y.Y*Y.Y) + (matrix.Y.Z*Z.Y), (matrix.Y.X*X.Z) + (matrix.Y.Y*Y.Z) + (matrix.Y.Z*Z.Z)),
				new Vector3((matrix.Z.X*X.X) + (matrix.Z.Y*Y.X) + (matrix.Z.Z*Z.X), (matrix.Z.X*X.Y) + (matrix.Z.Y*Y.Y) + (matrix.Z.Z*Z.Y), (matrix.Z.X*X.Z) + (matrix.Z.Y*Y.Z) + (matrix.Z.Z*Z.Z)));
		}

		public Matrix3 Multiply(Matrix2 matrix)
		{
			return new Matrix3(
				new Vector3((matrix.X.X*X.X) + (matrix.X.Y*Y.X), (matrix.X.X*X.Y) + (matrix.X.Y*Y.Y), X.Z),
				new Vector3((matrix.Y.X*X.X) + (matrix.Y.Y*Y.X), (matrix.Y.X*X.Y) + (matrix.Y.Y*Y.Y), Y.Z),
				Z);
		}
	
		public Matrix3 Normalize()
		{
			return new Matrix3(X.Normalize(), Y.Normalize(), Z.Normalize());
		}

		public Matrix3 NormalizeSafe()
		{
			return new Matrix3(X.NormalizeSafe(), Y.NormalizeSafe(), Z.NormalizeSafe());
		}
	
		public Matrix3 RotateAroundAxisX(Num radians)
		{
			Num tCos = (Num)MathS.Cos(-radians), tSin = (Num)MathS.Sin(-radians);
			return new Matrix3(
				X,
				new Vector3((Y.X*tCos) - (Z.X*tSin), (Y.Y*tCos) - (Z.Y*tSin), (Y.Z*tCos) - (Z.Z*tSin)),
				new Vector3((Y.X*tSin) + (Z.X*tCos), (Y.Y*tSin) + (Z.Y*tCos), (Y.Z*tSin) + (Z.Z*tCos)));
		}

		public Matrix3 RotateAroundAxisY(Num radians)
		{
			Num tCos = (Num)MathS.Cos(-radians), tSin = (Num)MathS.Sin(-radians);
			return new Matrix3(
				new Vector3((Z.X*tSin) + (X.X*tCos), (Z.Y*tSin) + (X.Y*tCos), (Z.Z*tSin) + (X.Z*tCos)),
				Y,
				new Vector3((Z.X*tCos) - (X.X*tSin), (Z.Y*tCos) - (X.Y*tSin), (Z.Z*tCos) - (X.Z*tSin)));
		}

		public Matrix3 RotateAroundAxisZ(Num radians)
		{
			Num tCos = (Num)MathS.Cos(-radians), tSin = (Num)MathS.Sin(-radians);
			return new Matrix3(
				new Vector3((X.X*tCos) - (Y.X*tSin), (X.Y*tCos) - (Y.Y*tSin), (X.Z*tCos) - (Y.Z*tSin)),
				new Vector3((X.X*tSin) + (Y.X*tCos), (X.Y*tSin) + (Y.Y*tCos), (X.Z*tSin) + (Y.Z*tCos)),
				Z);
		}

		public Matrix3 RotateAroundWorldAxisX(Num radians)
		{
			Num tCos = (Num)MathS.Cos(radians), tSin = (Num)MathS.Sin(radians);
			return new Matrix3(
				new Vector3(X.X, (X.Y*tCos) - (X.Z*tSin), (X.Y*tSin) + (X.Z*tCos)),
				new Vector3(Y.X, (Y.Y*tCos) - (Y.Z*tSin), (Y.Y*tSin) + (Y.Z*tCos)),
				new Vector3(Z.X, (Z.Y*tCos) - (Z.Z*tSin), (Z.Y*tSin) + (Z.Z*tCos)));
		}

		public Matrix3 RotateAroundWorldAxisY(Num radians)
		{
			Num tCos = (Num)MathS.Cos(radians), tSin = (Num)MathS.Sin(radians);
			return new Matrix3(
				new Vector3((X.Z*tSin) + (X.X*tCos), X.Y, (X.Z*tCos) - (X.X*tSin)),
				new Vector3((Y.Z*tSin) + (Y.X*tCos), Y.Y, (Y.Z*tCos) - (Y.X*tSin)),
				new Vector3((Z.Z*tSin) + (Z.X*tCos), Z.Y, (Z.Z*tCos) - (Z.X*tSin)));
		}

		public Matrix3 RotateAroundWorldAxisZ(Num radians)
		{
			Num tCos = (Num)MathS.Cos(radians), tSin = (Num)MathS.Sin(radians);
			return new Matrix3(
				new Vector3((X.X*tCos) - (X.Y*tSin), (X.X*tSin) + (X.Y*tCos), X.Z),
				new Vector3((Y.X*tCos) - (Y.Y*tSin), (Y.X*tSin) + (Y.Y*tCos), Y.Z),
				new Vector3((Z.X*tCos) - (Z.Y*tSin), (Z.X*tSin) + (Z.Y*tCos), Z.Z));
		}

		public Matrix3 Rotate(Vector3 vectorNormalized, Num radians)
		{
			return new Matrix3(X.Rotate(vectorNormalized, radians), Y.Rotate(vectorNormalized, radians), Z.Rotate(vectorNormalized, radians));
		}
		#endregion
	}
}